#include "interp.hpp"
#include <iostream>
#include <vector>
#include <cmath>

// Define time, displacement, and velocity data points for Hermite interpolation
std::vector<double> T = {0, 3, 5, 8, 13}; //correction: time -> T, avoid CE.
std::vector<double> displacement = {0, 225, 383, 623, 993};
std::vector<double> velocity = {75, 77, 80, 74, 72};

/**
 * @brief Predict the car's position at a given time using Hermite interpolation.
 * 
 * @param t The time at which to predict the position.
 * @param interp The Hermite interpolation object initialized with data points.
 * @return The predicted position at time t.
 */
double pred_Pos(double t, HermiteInterp interp) {
    return interp.evaluate(t);
}

/**
 * @brief Predict the car's velocity at a given time by approximating the derivative.
 * 
 * @param t The time at which to predict the velocity.
 * @param interp The Hermite interpolation object initialized with data points.
 * @return The predicted velocity at time t.
 */
double pred_V(double t, HermiteInterp interp) {
    return (interp.evaluate(t + 1e-6) - interp.evaluate(t - 1e-6)) / (2e-6); // Approximate derivative
}

int main() {
    // Initialize Hermite interpolation with time, displacement, and velocity data
    HermiteInterp interp(T, displacement, velocity);

    // Problem a: Predict position and velocity at t = 10 seconds
    std::cout << "Problem (a): At t = 10 seconds" << std::endl;
    std::cout << "Position is " << pred_Pos(10, interp) << " feet" << std::endl;
    std::cout << "Velocity is " << pred_V(10, interp) << " feet per second" << std::endl;

    // Problem b: Check if the car exceeds the speed limit of 81 feet per second over the interval
    std::cout << "Problem (b):" << std::endl;
    for (double t = 0; t <= 13; t += 0.1) {
        double V = pred_V(t, interp);
        if (V > 81) {
            std::cout << "The car EXCEEDS the speed limit of 81 feet per second at t = " << t << " seconds." << std::endl;
            return 0;
        }
    }
    
    // If speed limit is never exceeded
    std::cout << "The car NEVER exceeds the speed limit of 81 feet per second." << std::endl;
    return 0;
}
